#include "pwm.h"
#include <intrins.h>
#include "delay.h"

#define PWM_DIV 1 //
#define FPWM 10000 //

u16 code PWM_N = (MAIN_Fosc/FPWM/PWM_DIV); // <32767
u16 code PWM_WL = 0; // minimum reg. value
float code PWM_WA = MAIN_Fosc/FPWM/PWM_DIV/1023.0; // FULL

u16 pwm_set[2] = 0;     //8 bit
u16 pwm_act[2] = 0;     //10 bit

u8 hen_flag[2] = {1, 1};

void Pwm_Start()
{
    P4M0 |= 0x01;
    P4M1 &= ~0x01;

    P4M0 |= 0x02;
    P4M1 &= ~0x02;

	HEN0_PIN = 0; // CLOSE HEATER 
	PWM0_PIN = 1;
	HEN1_PIN = 0; // CLOSE HEATER 
	PWM1_PIN = 1;

    Delay_xms(3);

    PWM_init(PWM0_CH);
    PWM_init(PWM1_CH);
    Delay_xms(30);

    PWM_set(PWM0_CH, ~pwm_act[0]);
    PWM_set(PWM1_CH, ~pwm_act[1]);

    Delay_xms(100);
    HEN0_PIN = 1;
    HEN1_PIN = 1;
}

// 
void PWM_init(u8 channel) // 0 ~ 7
{
    channel &= 0x07;
    if(channel){
        channel = 1 << channel;
    }
    else{
        channel = 1;
    }
    P2M0 |= channel;
    P2M1 &= ~channel;

    PWMSET = 0x01; //
    P_SW2 |= 0x80;
    PWMCKS = PWM_DIV - 1;  // 
    PWMC = PWM_N; //
    P_SW2 &= 0x7f;
    PWMCFG = 0x01; //
}


void PWM_set(u8 channel, u16 power) // 10bit power
{
    u16 pwm_w;
    power &= 0x03ff;
    pwm_w = PWM_WL + ((u16)(PWM_WA * power));

    if(pwm_w >= PWM_N)
        pwm_w = PWM_N - 1;
    else if(pwm_w <= 1)
        pwm_w = 1;

    _push_(P_SW2); 
    P_SW2 |= 0x80; 
    switch(channel & 0x07)
    {
    case 0:
        PWM0T1 = pwm_w; 
        PWM0T2 = 0x0000; 
        PWM0CR = 0xC0; 
        break;
    case 1:
        PWM1T1 = pwm_w; 
        PWM1T2 = 0x0000; 
        PWM1CR = 0xC0; 
        break;
    }

    _pop_(P_SW2); 
}
